__version__ = "Sentry3 v0.0.2"
__license__ = "http://unlicense.org"

import ustruct  # pylint: disable=import-error
from time import sleep_ms  # pylint: disable=no-name-in-module

SENTRY_FIRMWARE_VERSION = 0xFF

SENTRY_MAX_RESULT = 25

SENTRY_OK = 0x00
SENTRY_FAIL = 0x01
SENTRY_WRITE_TIMEOUT = 0x02
SENTRY_READ_TIMEOUT = 0x03
SENTRY_CHECK_ERROR = 0x04
SENTRY_UNSUPPORT_PARAM = 0x10
SENTRY_UNKNOWN_PROTOCOL = 0x11

# Protocol Error Type
SENTRY_PROTOC_OK = 0xE0
SENTRY_PROTOC_FAIL = 0xE1
SENTRY_PROTOC_UNKNOWN = 0xE2
SENTRY_PROTOC_TIMEOUT = 0xE3
SENTRY_PROTOC_CHECK_ERROR = 0xE4
SENTRY_PROTOC_LENGTH_ERROR = 0xE5
SENTRY_PROTOC_UNSUPPORT_COMMAND = 0xE6
SENTRY_PROTOC_UNSUPPORT_REG_ADDRESS = 0xE7
SENTRY_PROTOC_UNSUPPORT_REG_VALUE = 0xE8
SENTRY_PROTOC_READ_ONLY = 0xE9
SENTRY_PROTOC_RESTART_ERROR = 0xEA
SENTRY_PROTOC_RESULT_NOT_END = 0xEC

# Protocol
SENTRY_PROTOC_START = 0xFF
SENTRY_PROTOC_END = 0xED
SENTRY_PROTOC_COMMADN_SET = 0x01
SENTRY_PROTOC_COMMADN_GET = 0x02
SENTRY_PROTOC_SET_PARAM = 0x21
SENTRY_PROTOC_GET_RESULT = 0x23
SENTRY_PROTOC_MESSAGE = 0x11
SENTRY_PROTOC_GET_STRING = 0x27


# sentry_reg
kRegDeviceId = 0x01
kRegFirmwareVersion = 0x02
kRegRestart = 0x03
kRegSensorConfig1 = 0x04
kRegLock = 0x05
kRegLed = 0x06
kRegLedLevel = 0x08
kRegUart = 0x09
kRegUSBCongig = 0x0B
kRegLcdCongig = 0x0C
kRegHWConfig = 0x0F
kRegCameraConfig1 = 0x10
kRegCameraConfig2 = 0x11
kRegCameraConfig3 = 0x12
kRegCameraConfig4 = 0x13
kRegCameraConfig5 = 0x14
kRegFrameWidthH = 0x1B
kRegFrameWidthL = 0x1C
kRegFrameHeightH = 0x1D
kRegFrameHeightL = 0x1E
kRegFrameCount = 0x1F
kRegVisionId = 0x20
kRegVisionConfig1 = 0x21
kRegVisionConfig2 = 0x22
kRegParamNum = 0x23
kRegParamId = 0x24
kRegVisionStatus1 = 0x2A
kRegVisionStatus2 = 0x2B
kRegVisionDetect1 = 0x30
kRegVisionDetect2 = 0x31
kRegResultNumber = 0x34
kRegResultId = 0x35
kRegReadStatus1 = 0x36
kRegStringNum = 0x6A
kRegStringData = 0x6C
kRegStringRxAll = 0x6E
kRegParamValue1H = 0x70
kRegParamValue1L = 0x71
kRegParamValue2H = 0x72
kRegParamValue2L = 0x73
kRegParamValue3H = 0x74
kRegParamValue3L = 0x75
kRegParamValue4H = 0x76
kRegParamValue4L = 0x77
kRegParamValue5H = 0x78
kRegParamValue5L = 0x79
kRegResultData1H = 0x80
kRegResultData1L = 0x81
kRegResultData2H = 0x82
kRegResultData2L = 0x83
kRegResultData3H = 0x84
kRegResultData3L = 0x85
kRegResultData4H = 0x86
kRegResultData4L = 0x87
kRegResultData5H = 0x88
kRegResultData5L = 0x89
kRegSn = 0xD0


# sentry_obj_info
class sentry_obj_info_e:
    kStatus = 1
    kXValue = 2
    kYValue = 3
    kWidthValue = 4
    kHeightValue = 5
    kLabel = 6
    kRValue = 7
    kGValue = 8
    kBValue = 9

# sentry_mode
class sentry_mode_e:
    kSerialMode = 0x00
    kI2CMode = 0x01
    kUnknownMode = 0x02

# sentry_led_color
class sentry_led_color_e:
    kLedClose = 0
    kLedRed = 1
    kLedGreen = 2
    kLedYellow = 3
    kLedBlue = 4
    kLedPurple = 5
    kLedCyan = 6
    kLedWhite = 7

# Sentry color label
class color_label_e:
    kColorBlack = 1
    kColorWhite = 2
    kColorRed = 3
    kColorGreen = 4
    kColorBlue = 5
    kColorYellow = 6

class apriltag_vision_mode_e:
    kVisionModeFamily16H5 = 0
    kVisionModeFamily25H9 = 1
    kVisionModeFamily36H11 = 2

# sentry3 vision
class sentry3_vision_e:
    kVisionColor = 1
    kVisionBlob = 2
    kVisionAprilTag = 3
    kVisionLine = 4
    kVisionLearning = 5
    kVisionFace = 7
    kVisionYolo = 8
    kVisionQrCode = 9
    kVisionBarCode = 10
    kVisionOCR = 12
    kVisionHandPose = 13
    kVisionLicensePlate = 14
    kVisionMaxType = 15



class yolo_label_e:
    kPerson = 0
    kBicycle = 1
    kCar = 2
    kMotorcycle = 3
    kAirplane = 4
    kBus = 5
    kTrain = 6
    kTruck = 7
    kBoat = 8
    kTrafficLight = 9
    kFireHydrant = 10
    kStopSign = 11
    kParkingMeter = 12
    kBench = 13
    kBird = 14
    kCat = 15
    kDog = 16
    kHorse = 17
    kSheep = 18
    kCow = 19
    kElephant = 20
    kBear = 21
    kZebra = 22
    kGiraffe = 23
    kBackpack = 24
    kUmbrella = 25
    kHandbag = 26
    kTie = 27
    kSuitcase = 28
    kFrisbee = 29
    kSkis = 30
    kSnowboard = 31
    kSportsBall = 32
    kKite = 33
    kBaseballBat = 34
    kBaseballGlove = 35
    kSkateboard = 36
    kSurfboard = 37
    kTennisRacket = 38
    kBottle = 39
    kWineGlass = 40
    kCup = 41
    kFork = 42
    kKnife = 43
    kSpoon = 44
    kBowl = 45
    kBanana = 46
    kApple = 47
    kSandwich = 48
    kOrange = 49
    kBroccoli = 50
    kCarrot = 51
    kHotDog = 52
    kPizza = 53
    kDonut = 54
    kCake = 55
    kChair = 56
    kCouch = 57
    kPottedPlant = 58
    kBed = 59
    kDiningTable = 60
    kToilet = 61
    kTv = 62
    kLaptop = 63
    kMouse = 64
    kRemote = 65
    kKeyboard = 66
    kCellPhone = 67
    kMicrowave = 68
    kOven = 69
    kToaster = 70
    kSink = 71
    kRefrigerator = 72  
    kBook = 73
    kClock = 74
    kVase = 75
    kScissors = 76
    kTeddyBear = 77
    kHairDrier = 78
    kToothbrush = 79



LOG_OFF = 60
LOG_CRITICAL = 50
LOG_ERROR = 40
LOG_WARNING = 30
LOG_INFO = 20
LOG_DEBUG = 10
LOG_NOTSET = 0

global __level__
class SentryLogger:
    global __level__
    __level__ = LOG_INFO
    _level_dict = {
        LOG_CRITICAL: "CRIT",
        LOG_ERROR: "ERROR",
        LOG_WARNING: "WARN",
        LOG_INFO: "INFO",
        LOG_DEBUG: "DEBUG",
    }

    def _level_str(self, level):
        l = self._level_dict.get(level)
        if l is not None:
            return l
        return "LVL%s" % level

    def setLevel(self, level):
        global __level__
        __level__ = level

    def isEnabledFor(self, level):
        global __level__
        return level >= __level__

    def log(self, name, level, msg, *args):
        if self.isEnabledFor(level):
            levelname = self._level_str(level)
            msgformat = ["%s.%s:" % (name, levelname)]
            len_arg = len(args)

            if type(msg) == type("str") and len_arg > 0:
                len_msg = msg.count('%')
                if len_msg >= len_arg and len_msg > 0:
                    msgformat.append(msg % args)
                else:
                    msgformat.append(msg)
                    msgformat += args
            else:
                msgformat.append(msg)
                msgformat += args

            print(*msgformat, sep=" ")


class result:
    result_data1 = 0
    result_data2 = 0
    result_data3 = 0
    result_data4 = 0
    result_data5 = 0
    bytestr = ""


class VisionState:
    def __init__(self, vision_type):
        self.vision_type = vision_type
        self.frame = 0
        self.detect = 0
        self.result = []

        for _ in range(SENTRY_MAX_RESULT):
            self.result.append(result())


class SentryI2CMethod:
    def __init__(self, address, communication_port, logger=None):
        self.__mu_address = address
        self.__communication_port = communication_port
        self.__logger = logger

        if address not in communication_port.scan():
            raise ValueError(
                "SentryI2CMethod Init Error!!! address %#x cannot found!" % address)

    def Logger(self, *arg):  # level, format, args
        if self.__logger:
            self.__logger(self.__class__.__name__, *arg)

    def Set(self, reg_address, value):
        data = ustruct.pack("<b", value)
        self.__communication_port.writeto_mem(
            self.__mu_address, reg_address, data)

        self.Logger(LOG_DEBUG, "set-> reg:%#x var:%#x",
                    reg_address, value)

        return SENTRY_OK

    def Get(self, reg_address):
        data = ustruct.pack("<b", reg_address)
        self.__communication_port.writeto(self.__mu_address, data)

        value = self.__communication_port.readfrom(
            self.__mu_address, 1)
        if value:
            self.Logger(LOG_DEBUG, "Get-> reg:%#x var:%#x",
                        reg_address, value[0])
            return (SENTRY_OK, value[0])
        else:
            self.Logger(LOG_ERROR, "Get-> reg:%#x TimeOut!",
                        reg_address)

            return (SENTRY_READ_TIMEOUT, 0)

    def __get_result_data(self, kRegResultDataL, kRegResultDataH):
        err, result_data_tmp1 = self.Get(kRegResultDataL)
        if err:
            return (err, 0)
        err, result_data_tmp2 = self.Get(kRegResultDataH)
        if err:
            return (err, 0)

        return (err, result_data_tmp2 << 8 | result_data_tmp1)

    def Read(self, vision_type, vision_state):
        err = self.Set(kRegVisionId, vision_type)
        if err:
            return (err, vision_state)

        err, vision_state.frame = self.Get(kRegFrameCount)
        if err:
            return (err, vision_state)

        err, vision_state.detect = self.Get(kRegResultNumber)
        if err:
            return (err, vision_state)

        if not vision_state.detect:
            return (SENTRY_OK, vision_state)

        vision_state.detect = SENTRY_MAX_RESULT if SENTRY_MAX_RESULT < vision_state.detect else vision_state.detect

        for i in range(vision_state.detect):
            err = self.Set(kRegResultId, i+1)
            if err:
                return (err, vision_state)

            err, vision_state.result[i].data1 = self.__get_result_data(
                kRegResultData1L, kRegResultData1H)
            if err:
                return (err, vision_state)
            err, vision_state.result[i].data2 = self.__get_result_data(
                kRegResultData2L, kRegResultData2H)
            if err:
                return (err, vision_state)
            err, vision_state.result[i].data3 = self.__get_result_data(
                kRegResultData3L, kRegResultData3H)
            if err:
                return (err, vision_state)
            err, vision_state.result[i].data4 = self.__get_result_data(
                kRegResultData4L, kRegResultData4H)
            if err:
                return (err, vision_state)
            err, vision_state.result[i].data5 = self.__get_result_data(
                kRegResultData5L, kRegResultData5H)
            if err:
                return (err, vision_state)

        return (SENTRY_OK, vision_state)

    def SetParam(self, vision_id, param, param_id):
        err = self.Set(kRegVisionId, vision_id)
        if err:
            return err

        err = self.Set(kRegParamId, param_id)
        if err:
            return err

        self.Set(kRegParamValue1H, param[0])
        self.Set(kRegParamValue1L, param[1])
        self.Set(kRegParamValue2H, param[2])
        self.Set(kRegParamValue2L, param[3])
        self.Set(kRegParamValue3H, param[4])
        self.Set(kRegParamValue3L, param[5])
        self.Set(kRegParamValue4H, param[6])
        self.Set(kRegParamValue4L, param[7])
        self.Set(kRegParamValue5H, param[8])
        self.Set(kRegParamValue5L, param[9])

        return SENTRY_OK

    def ReadString(self, vision_type, obj_id, vision_state):
        err = self.Set(kRegVisionId, vision_type)
        if err:
            return err
        err = self.Set(kRegResultId, obj_id)
        if err:
            return err
        err, str_len = self.Get(kRegStringNum)
        if err:
            return err
        err = self.Set(kRegStringRxAll, 1)
        if err:
            return err

        value = bytes()
        for _ in range(str_len):
            data = ustruct.pack('<b', kRegStringData)
            self.__communication_port.writeto(self.__mu_address, data)
            value += self.__communication_port.readfrom(
                self.__mu_address, 1)
        print(value)
        vision_state.result[obj_id-1].bytestr = value.decode('utf-8','replace')

        return SENTRY_OK

class SentryUartMethod:
    """

    """

    def __init__(self, address, communication_port, logger=None):
        self.__mu_address = address
        self.__communication_port = communication_port
        self.__logger = logger
        # Setting serial port parameters
        self.__communication_port.init(timeout=1000, timeout_char=10)

    def Logger(self, *arg):  # level, format, args
        if self.__logger:
            self.__logger(self.__class__.__name__, *arg)

    def __cheak(self, data):
        count = 0
        for i in data[:-2]:
            count += i
        count &= 0xff

        if count == data[-2]:
            return SENTRY_PROTOC_OK
        else:
            return SENTRY_PROTOC_CHECK_ERROR

    def __protocol_read(self):

        count_ms = 0
        # The shortest receiving time of serial protocol is 6 bytes
        while self.__communication_port.any() < 6:
            count_ms += 1
            # The maximum waiting time for receiving data is 1s
            if count_ms < 1000:
                sleep_ms(1)
            else:
                self.Logger(LOG_ERROR, "Waiting for reception timeOut!!!")
                return (SENTRY_PROTOC_TIMEOUT, [])

        self.Logger(LOG_DEBUG, "Waiting for reception takes %dms", count_ms)

        data_len = 0
        data_list = []
        for _ in range(self.__communication_port.any()):
            data_list.append(self.__communication_port.read(1)[0])
            if data_list[0] == SENTRY_PROTOC_START:
                data_list.append(self.__communication_port.read(1)[0])
                data_len = data_list[1]
                data_list += list(self.__communication_port.read(data_len-2))
                break

        if self.__logger:
            self.Logger(LOG_DEBUG, "    rev-> %s",
                        ' '.join(['%02x' % b for b in data_list]))

        if data_len > 0 and data_len != len(data_list):
            return (SENTRY_PROTOC_CHECK_ERROR, [])

        if SENTRY_PROTOC_END != data_list[-1]:
            return (SENTRY_PROTOC_CHECK_ERROR, [])

        if self.__cheak(data_list) != SENTRY_PROTOC_OK:
            return (SENTRY_PROTOC_CHECK_ERROR, [])

        return (SENTRY_PROTOC_OK, data_list[3:-2])

    def Set(self, reg_address, value):

        data_list = [SENTRY_PROTOC_START, 0, self.__mu_address,
                     SENTRY_PROTOC_COMMADN_SET, reg_address, value]
        data_list[1] = len(data_list)+2
        cheak_num = 0
        for da in data_list:
            cheak_num += da

        data_list.append(cheak_num & 0xff)
        data_list.append(SENTRY_PROTOC_END)

        data = ustruct.pack(">"+"b"*len(data_list), *tuple(data_list))

        if self.__logger:
            self.Logger(LOG_DEBUG, "Set req-> %s",
                        ' '.join(['%02x' % b for b in data]))

        if self.__communication_port.any():
            # Clear cache before sending
            self.__communication_port.read()
        self.__communication_port.write(data)

        try_time = 0
        while True:
            err, data = self.__protocol_read()
            if err == SENTRY_PROTOC_OK:
                if data[0] == SENTRY_PROTOC_OK or \
                        data[1] == SENTRY_PROTOC_COMMADN_GET or \
                        data[2] == reg_address:
                    return SENTRY_OK
                else:
                    return data[0]

            elif err == SENTRY_PROTOC_TIMEOUT:
                try_time += 1
                if try_time > 3:
                    return SENTRY_READ_TIMEOUT
            else:
                return SENTRY_FAIL

    def Get(self, reg_address):

        data_list = [SENTRY_PROTOC_START, 0, self.__mu_address,
                     SENTRY_PROTOC_COMMADN_GET, reg_address]
        data_list[1] = len(data_list)+2
        cheak_num = 0
        for da in data_list:
            cheak_num += da

        data_list.append(cheak_num & 0xff)
        data_list.append(SENTRY_PROTOC_END)

        data = ustruct.pack(">"+"b"*len(data_list), *tuple(data_list))

        if self.__logger:
            self.Logger(LOG_DEBUG, "Get req-> %s",
                        ' '.join(['%02x' % b for b in data]))

        if self.__communication_port.any():
            # Clear cache before sending
            self.__communication_port.read()
        self.__communication_port.write(data)

        try_time = 0
        while True:
            err, data = self.__protocol_read()
            if err == SENTRY_PROTOC_OK:
                if data[0] == SENTRY_PROTOC_OK or \
                        data[1] == SENTRY_PROTOC_COMMADN_GET:
                    return (SENTRY_OK, data[2])
                else:
                    return (data[0], 0)

            elif err == SENTRY_PROTOC_TIMEOUT:
                try_time += 1
                if try_time > 3:
                    return SENTRY_READ_TIMEOUT
            else:
                return SENTRY_FAIL

    def Read(self, vision_type, vision_state):

        data_list = [SENTRY_PROTOC_START, 0, self.__mu_address,
                     SENTRY_PROTOC_GET_RESULT, vision_type, 0, 0]
        data_list[1] = len(data_list)+2
        cheak_num = 0
        for da in data_list:
            cheak_num += da

        data_list.append(cheak_num & 0xff)
        data_list.append(SENTRY_PROTOC_END)

        data = ustruct.pack(">"+"b"*len(data_list), *tuple(data_list))

        if self.__logger:
            self.Logger(LOG_DEBUG, "Read req-> %s",
                        ' '.join(['%02x' % b for b in data]))

        if self.__communication_port.any():
            # Clear cache before sending
            self.__communication_port.read()
        self.__communication_port.write(data)

        try_time = 0
        vision_state.detect = 0

        while True:
            err, data = self.__protocol_read()
            #print("read",hex(err), hex(data[0]))
            if err == SENTRY_PROTOC_OK:
                if data[0] == SENTRY_PROTOC_OK or data[0] == SENTRY_PROTOC_RESULT_NOT_END:
                    if data[1] == SENTRY_PROTOC_GET_RESULT and data[3] == vision_type:
                        vision_state.frame = data[2]
                        start_id = data[4]
                        stop_id = data[5]

                        if SENTRY_MAX_RESULT < stop_id:
                            return (SENTRY_UNSUPPORT_PARAM, vision_state)

                        if not start_id:
                            return (SENTRY_OK, vision_state)

                        vision_state.detect = stop_id-start_id+1

                        for i in range(vision_state.detect):
                            v_id = i+start_id-1
                            vision_state.result[v_id].data1 = data[10 *
                                                                   i + 6] << 8 | data[10 * i + 7]
                            vision_state.result[v_id].data2 = data[10 *
                                                                   i + 8] << 8 | data[10 * i + 9]
                            vision_state.result[v_id].data3 = data[10 *
                                                                   i + 10] << 8 | data[10 * i + 11]
                            vision_state.result[v_id].data4 = data[10 *
                                                                   i + 12] << 8 | data[10 * i + 13]
                            vision_state.result[v_id].data5 = data[10 *
                                                                   i + 14] << 8 | data[10 * i + 15]

                        if data[0] == SENTRY_PROTOC_RESULT_NOT_END:
                            continue
                        else:
                            return (SENTRY_OK, vision_state)
                    else:
                        return (SENTRY_UNSUPPORT_PARAM, vision_state)
            elif err == SENTRY_PROTOC_TIMEOUT:
                try_time += 1
                if try_time > 3:
                    return (SENTRY_READ_TIMEOUT, vision_state)
            else:
                 return (SENTRY_FAIL, vision_state)

    def SetParam(self, vision_id, param: list, param_id):
        data_list = [SENTRY_PROTOC_START, 0, self.__mu_address,
                     SENTRY_PROTOC_SET_PARAM, vision_id, param_id, param_id]

        data_list += param
        data_list[1] = len(data_list)+2
        cheak_num = 0
        for da in data_list:
            cheak_num += da

        data_list.append(cheak_num & 0xff)
        data_list.append(SENTRY_PROTOC_END)

        data = ustruct.pack(">"+"b"*len(data_list), *tuple(data_list))

        if self.__logger:
            self.Logger(LOG_DEBUG, "Set req-> %s",
                        ' '.join(['%02x' % b for b in data]))

        if self.__communication_port.any():
            # Clear cache before sending
            self.__communication_port.read()
        self.__communication_port.write(data)

        try_time = 0

        while True:
            err, data = self.__protocol_read()

            if err == SENTRY_PROTOC_OK:
                if data[0] == SENTRY_PROTOC_OK:
                    if data[1] == SENTRY_PROTOC_SET_PARAM:
                        # FIXME: which is right?
                        # if (ret_val.buf[2] == vision_type:
                        return SENTRY_OK
                    # else:
                    #    return SENTRY_FAIL
                    else:
                        return SENTRY_UNSUPPORT_PARAM
                else:
                    return SENTRY_READ_TIMEOUT
            elif err == SENTRY_PROTOC_TIMEOUT:
                try_time += 1
                if try_time > 3:
                    return SENTRY_READ_TIMEOUT
            else:
                 return SENTRY_FAIL

    def ReadString(self, vision_type, obj_id, vision_state):
        data_list = [SENTRY_PROTOC_START, 0, self.__mu_address,
                     SENTRY_PROTOC_GET_STRING, vision_type, obj_id, 0, 0]
        data_list[1] = len(data_list)+2
        cheak_num = 0
        for da in data_list:
            cheak_num += da
        data_list.append(cheak_num & 0xff)
        data_list.append(SENTRY_PROTOC_END)
        data = ustruct.pack(">"+"b"*len(data_list), *tuple(data_list))
        if self.__logger:
            self.Logger(LOG_DEBUG, "ReadString req-> %s",
                        ' '.join(['%02x' % b for b in data]))
        if self.__communication_port.any():
            # Clear cache before sending
            self.__communication_port.read()
        self.__communication_port.write(data)

        try_time = 0
        str_data = bytearray()  # 用于累积分片数据
        
        while True:
            err, data = self.__protocol_read()
            if err == SENTRY_PROTOC_OK:
                if ((data[0] == SENTRY_PROTOC_OK or data[0] == SENTRY_PROTOC_RESULT_NOT_END) and 
                    data[3] == vision_type):             
                    if (data[1] == SENTRY_PROTOC_GET_STRING and data[4] == obj_id):
                        start_char_idx = data[5]
                        stop_char_idx = data[6]
                        str_len = stop_char_idx - start_char_idx + 1
                        if str_len != len(data) - 7:
                            if self.__logger:
                                self.Logger(LOG_ERROR, "Assert string length fail: %d vs %d", 
                                           str_len, len(data) - 7)
                            return SENTRY_CHECK_ERROR
                        if len(data) > 7:
                            for s in data[7:]:
                                str_data.append(s)
                        if data[0] == SENTRY_PROTOC_OK:
                            break
                        else:
                            continue
                    else:
                        if self.__logger:
                            self.Logger(LOG_ERROR, "Error command[0x%x], result_id[%d]", 
                                       data[1], data[4])
                        return SENTRY_UNSUPPORT_PARAM
                else:
                    return SENTRY_UNSUPPORT_PARAM
            elif err == SENTRY_PROTOC_TIMEOUT:
                try_time += 1
                if try_time > 3:
                    return SENTRY_READ_TIMEOUT
            else:
                return SENTRY_FAIL
        
        if len(str_data) > 0:
            vision_state.result[obj_id-1].bytestr = str_data.decode('utf-8','replace')
        
        return SENTRY_OK

class SentryBase:
    """

    """

    def __init__(self,device_id, address=0x60, log_level=LOG_ERROR):
        self.__device_id = device_id
        self.__address = address
        self.__stream = None
        self.__img_w = 0
        self.__img_h = 0
        self.__debug = None
        self.__vision_states = [None]*sentry3_vision_e.kVisionMaxType

        self.SetDebug(log_level)

    def Logger(self, *arg):  # level, format, args
        if self.__logger:
            self.__logger(self.__class__.__name__, *arg)

    def SetDebug(self, log_level=LOG_OFF):
        if log_level < LOG_OFF:
            self.__debug = SentryLogger()
            self.__logger = self.__debug.log
            self.__debug.setLevel(log_level)
        else:
            if self.__debug:
                self.__debug.setLevel(LOG_OFF)

    def __SensorLockkReg(self, lock: bool):
        return self.__stream.Set(kRegLock, lock)

    def __SensorStartupCheck(self):
        err_count = 0
        while True:
            err_count += 1
            err, start_up = self.__stream.Get(kRegSensorConfig1)
            if err:
                self.Logger(LOG_ERROR, "SensorStartupCheck error:%d!"%err)
                return err

            if  start_up & 0x01:
                break

            sleep_ms(10)
            if err_count > 200:
                self.Logger(LOG_ERROR, "SensorStartupCheck error!")
                return SENTRY_UNKNOWN_PROTOCOL

        return SENTRY_OK
        
    def __ProtocolVersionCheck(self):
        err_count = 0
        while True:
            err_count += 1
            err, protocol_version = self.__stream.Get(kRegDeviceId)
            if (not err) and protocol_version == self.__device_id:
                break
            if err_count > 3:
                self.Logger(LOG_ERROR, "ProtocolVersionCheck error!")
                return SENTRY_UNKNOWN_PROTOCOL

        return err

    def GetImageShape(self):
        tmp = [0, 0]
        err, tmp[0] = self.__stream.Get(kRegFrameWidthL)
        if err:
            return err
        err, tmp[1] = self.__stream.Get(kRegFrameWidthH)
        if err:
            return err
        self.__img_w = tmp[1] << 8 | tmp[0]
        err, tmp[0] = self.__stream.Get(kRegFrameHeightL)
        if err:
            return err
        err, tmp[1] = self.__stream.Get(kRegFrameHeightH)
        if err:
            return err
        self.__img_h = tmp[1] << 8 | tmp[0]

        return SENTRY_OK

    def rows(self):
        return self.__img_h

    def cols(self):
        return self.__img_w

    def SensorInit(self):

        # Check sensor startup
        err = self.__SensorStartupCheck()
        if err:
            return err
        # Check sentry protocol version
        err = self.__ProtocolVersionCheck()
        if err:
            return err
        # Sensor set default if version is correction.
        err = self.SensorSetDefault()
        if err:
            return err
        # Get sensor image shape.
        err = self.GetImageShape()
        if err:
            return err

        return SENTRY_OK

    def begin(self, communication_port=None):
        if "I2C" in communication_port.__class__.__name__:
            self.__stream = SentryI2CMethod(
                self.__address, communication_port, logger=self.__logger)
            self.Logger(LOG_INFO, "Begin I2C mode succeed!")

        elif 'UART' == communication_port.__class__.__name__:
            self.__stream = SentryUartMethod(
                self.__address, communication_port, logger=self.__logger)
            self.Logger(LOG_INFO, "Begin UART mode succeed!")

        elif communication_port == None:
            from machine import I2C, Pin  # pylint: disable=import-error
            communication_port = I2C(
                scl=Pin(Pin.P19), sda=Pin(Pin.P20), freq=400000)
            return self.begin(communication_port)

        else:
            return SENTRY_UNSUPPORT_PARAM

        if self.__stream:
            return self.SensorInit()

        return SENTRY_FAIL

    def VisionBegin(self, vision_type):
        err = self.VisionSetStatus(vision_type, True)
        if err:
            return err

        return SENTRY_OK

    def VisionEnd(self, vision_type):
        return self.VisionSetStatus(vision_type, False)

    def GetValue(self, vision_type, object_inf, obj_id=1):
        '''
         Note: when getting the vision status, if the block is true, it will wait until the vision_type result is updated   
        '''
        if object_inf == sentry_obj_info_e.kStatus:
            err = True
            while err:
                err = self.UpdateResult(vision_type)
                sleep_ms(10)  # pylint: disable=undefined-variable

        return self.__read(vision_type, object_inf, obj_id)

    def SetParamNum(self, vision_type, max_num):
        err = self.__stream.Set(kRegVisionId, vision_type)
        if err:
            return err

        err = self.__stream.Set(kRegParamNum, max_num)

        return err

    def SetParam(self, vision_type, param: list, param_id):
        if param_id < 0 or param_id > SENTRY_MAX_RESULT:
            return SENTRY_FAIL

        params = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        for i in range(min(len(param), 5)):
            params[i*2] = param[i] >> 8
            params[i*2+1] = param[i] & 0xff

        return self.__stream.SetParam(vision_type, params, param_id)

    def VisionSetMode(self, vision_type, mode):

        err = self.__stream.Set(kRegVisionId, vision_type);
        if err:
            return err
        err, vision_config_reg_value = self.__stream.Get(kRegVisionConfig2)
        if err:
            return err
        
        _mode = vision_config_reg_value&0x0f
        if _mode != mode:
            vision_config_reg_value &= 0xf0
            vision_config_reg_value |= mode
            
            err = self.__stream.Set(kRegVisionConfig2, vision_config_reg_value)
        return err;


    def GetVisionState(self, vision_type):
        if vision_type >= sentry3_vision_e.kVisionMaxType:
            return 0

        return self.__vision_states[vision_type-1]

    def VisionSetStatus(self, vision_type, enable: bool):

        err = self.__stream.Set(kRegVisionId, vision_type)
        if err:
            return err

        err, vision_config_reg_value = self.__stream.Get(
            kRegVisionConfig1)
        if err:
            return err

        status = vision_config_reg_value & 0x01
        if status != enable:
            vision_config_reg_value &= 0xfe
            vision_config_reg_value |= enable & 0x01

            err = self.__stream.Set(
                kRegVisionConfig1, vision_config_reg_value)
            if err:
                return err

        if enable:
            self.__vision_states[vision_type-1] = VisionState(vision_type)

        else:
            self.__vision_states[vision_type-1] = None

    def VisionSetDefault(self, vision_type):

        err = self.__stream.Set(kRegVisionId, vision_type)
        if err:
            return err
        err, vision_config_reg_value = self.__stream.Get(
            kRegVisionConfig1)
        if err:
            return err

        vision_config_reg_value &= 0xfd
        vision_config_reg_value |= 0x01 << 1
        default_setting = (vision_config_reg_value >> 1) & 0x01
        err = self.__stream.Set(kRegVisionConfig1,
                                vision_config_reg_value)
        if err:
            return err

        while default_setting:

            sleep_ms(10)

            err, vision_config_reg_value = self.__stream.Get(
                kRegVisionConfig1)
            if err:
                return err
            default_setting = (vision_config_reg_value >> 1) & 0x01

        return SENTRY_OK

    def VisionGetStatus(self, vision_type):
        err = self.__stream.Set(kRegVisionId, vision_type)
        if err:
            return 0
 
        err, vision_status1 = self.__stream.Get(
            kRegVisionConfig1)

        if err:
            return 0

        return 0x01 & vision_status1

    def UpdateResult(self, vision_type):

        if vision_type >= sentry3_vision_e.kVisionMaxType:
            return 0

        vision_state = self.__vision_states[vision_type-1]

        while SENTRY_OK != self.__SensorLockkReg(False):
            pass

        err, frame = self.__stream.Get(kRegFrameCount)
        if err:
            return err

        if frame == vision_state.frame:
            return SENTRY_FAIL

        while SENTRY_OK != self.__SensorLockkReg(True):
            pass

        try:
            err, vision_state = self.__stream.Read(vision_type, vision_state)
        finally:
            while SENTRY_OK != self.__SensorLockkReg(False):
                pass

        self.__vision_states[vision_type-1] = vision_state

        if err:
            return err

        return SENTRY_OK

    def __read(self, vision_type, object_inf, obj_id):

        if vision_type >= sentry3_vision_e.kVisionMaxType:
            return 0

        vision_state = self.__vision_states[vision_type-1]
        if obj_id < 1 or obj_id >SENTRY_MAX_RESULT or vision_state == None :
            return 0

        obj_id -= 1

        if vision_state.detect <= obj_id:
            return 0

        if object_inf == sentry_obj_info_e.kStatus:
            return vision_state.detect
        elif object_inf == sentry_obj_info_e.kXValue:
            return vision_state.result[obj_id].data1
        elif object_inf == sentry_obj_info_e.kYValue:
            return vision_state.result[obj_id].data2
        elif object_inf == sentry_obj_info_e.kWidthValue:
            return vision_state.result[obj_id].data3
        elif object_inf == sentry_obj_info_e.kHeightValue:
            return vision_state.result[obj_id].data4
        elif object_inf == sentry_obj_info_e.kLabel:
            return vision_state.result[obj_id].data5
        elif object_inf == sentry_obj_info_e.kRValue:
            return vision_state.result[obj_id].data1
        elif object_inf == sentry_obj_info_e.kGValue:
            return vision_state.result[obj_id].data2
        elif object_inf == sentry_obj_info_e.kBValue:
            return vision_state.result[obj_id].data3
        else:
            return 0

    def GetString(self, vision_type, obj_id=1):
        vision_state = self.__vision_states[vision_type-1]
        if vision_state == None or obj_id < 1 or obj_id >SENTRY_MAX_RESULT:
            return ''
        err = self.__stream.ReadString(vision_type, obj_id, vision_state)
        if err:
            return ''
            
        return vision_state.result[obj_id-1].bytestr

    def SensorSetRestart(self):
        err = self.__stream.Set(kRegRestart, 1)
        if err:
            return err

        return SENTRY_OK

    def SensorSetDefault(self):
        err, sensor_config_reg_value = self.__stream.Get(kRegSensorConfig1)
        if err:
            return err

        sensor_config_reg_value |= 0x08

        err = self.__stream.Set(kRegSensorConfig1,
                                sensor_config_reg_value)
        while True:
            err, sensor_config_reg_value = self.__stream.Get(
                kRegSensorConfig1)
            if err:
                return err

            if not (sensor_config_reg_value & 0x08):
                self.Logger(LOG_INFO, "SensorSetDefault succeed!")
                break

        return err

    def LedSetColor(self, detected_color, undetected_color, level):

        err, led_level = self.__stream.Get(kRegLedLevel)
        if err:
            return err

        led_level &= 0xF0
        led_level |= (level & 0x0F)
        self.__stream.Set(kRegLedLevel, led_level)

        err, led_reg_value = self.__stream.Get(kRegLed)
        if err:
            return err

        led_reg_value &= 0x10

        if detected_color == undetected_color:
            led_reg_value |= 0x01      

        led_reg_value |= (detected_color & 0x07) << 1
        led_reg_value |= (undetected_color & 0x07) << 5

        err = self.__stream.Set(kRegLed, led_reg_value)
        if err:
            return err

        return SENTRY_OK

    def LcdSetMode(self, on):

        err, lcd_reg_value = self.__stream.Get(kRegLcdCongig)
        if err:
            return err

        lcd_reg_value &= 0xFe
        lcd_reg_value |= (on & 0x01)

        err = self.__stream.Set(kRegLcdCongig, lcd_reg_value)
        if err:
            return err

        return SENTRY_OK

class Sentry3(SentryBase):
    SENTRY3_DEVICE_ID = 0x08
    def __init__(self, address=0x60, log_level=LOG_ERROR):
        super().__init__(self.SENTRY3_DEVICE_ID,address,log_level)
